function y=RRPdy(x)
%Dynamic analysis for RRP bar group
%Copyright 2003 Qu Xiuquan
%Input parameters
%x(1)=theta-i
%x(2)=dtheta-i
%x(3)=ddtheta-i
%x(4)=dd-j
%x(5)=Re[ddB]
%x(6)=Im[ddB]
%Output parameters
%
%y(1)=RxB
%y(2)=RyB
%y(3)=RxC
%y(4)=RyC
%y(5)=RD
%
g=9.8;
ri=1.2;rci=0.6;mi=3.6;mj=6;Ji=0.45;
Fxi=0;Fyi=0;Fxj=1000,Fyi=0;Mi=0;
thj=0;
%
a =zeros(5);
a(1,1)=1;a(1,3)=1;
a(2,2)=1;a(2,4)=1;
a(3,1)=rci*sin(x(1));a(3,2)=-rci*cos(x(1));a(3,3)=-(ri-rci)*sin(x(1));
a(3,4)=(ri-rci)*cos(x(1));
a(4,3)=-1;a(4,5)=-sin(thj);
a(5,4)=-1;a(5,5)=cos(thj)
b=zeros(5,1);
b(1,1)=mi*x(5)+mi*rci*x(3)*cos(x(1)+pi/2)+mi*rci*x(2)^2*cos(x(1)+pi)-Fxi;
b(2,1)=mi*x(6)+mi*rci*x(3)*sin(x(1)+pi/2)+mi*rci*x(2)^2*sin(x(1)+pi)-Fyi+mi*g;
b(3,1)=Ji*x(3)-Mi;
b(4,1)=mj*x(4)*cos(thj)-Fxj;
b(5,1)=mj*x(4)*sin(thj)-Fxj+mj*g;
y=inv(a)*b;
end

